
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_motors.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_motors.h"

#include <mavproxy/mavproxy.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       电机模块初始化
  * @param[in]   pMotors  
  * @param[in]   loop_rate  
  * @param[in]   speed_hz  
  * @param[out]  
  * @retval      
  * @note        
  */
void Motors(Motors_HandleTypeDef *pMotors, uint16_t loop_rate, uint16_t speed_hz)
{
    pMotors->_loop_rate = loop_rate;
    pMotors->_speed_hz  = speed_hz;
    pMotors->_spool_desired  = MOTOR_DESIRED_SHUT_DOWN;
    pMotors->_spool_state    = MOTOR_SHUT_DOWN;
    pMotors->_air_density_ratio    = 1.0f;

    lpf_set_cutoff1(&pMotors->_throttle_filter, 0.0f);
    lpf_reset(&pMotors->_throttle_filter, 0.0f);

    // init limit flags
    pMotors->limit.roll = true;
    pMotors->limit.pitch = true;
    pMotors->limit.yaw = true;
    pMotors->limit.throttle_lower = true;
    pMotors->limit.throttle_upper = true;
    pMotors->_thrust_boost = false;
    pMotors->_thrust_balanced = true;
}

void Motors_set_desired_spool_state(Motors_HandleTypeDef *pMotors, DesiredSpoolState spool)
{
    if (pMotors->_armed || (spool == MOTOR_DESIRED_SHUT_DOWN)) {
        pMotors->_spool_desired = spool;
    }
}

DesiredSpoolState Motors_get_desired_spool_state(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_spool_desired;
}

SpoolState Motors_get_spool_state(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_spool_state;
}

// set_density_ratio - sets air density as a proportion of sea level density
void Motors_set_air_density_ratio(Motors_HandleTypeDef *pMotors, float ratio)
{
    pMotors->_air_density_ratio = ratio;
}

// set limit flag for pitch, roll and yaw
void Motors_set_limit_flag_pitch_roll_yaw(Motors_HandleTypeDef *pMotors, bool flag)
{
    pMotors->limit.roll  = flag;
    pMotors->limit.pitch = flag;
    pMotors->limit.yaw   = flag;
}

// set update rate to motors - a value in hertz
void Motors_set_update_rate(Motors_HandleTypeDef *pMotors, uint16_t speed_hz )
{
    pMotors->_speed_hz = speed_hz;
}

// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
void Motors_set_radio_passthrough(Motors_HandleTypeDef *pMotors, float roll_input, float pitch_input, float throttle_input, float yaw_input)
{
    pMotors->_roll_radio_passthrough = roll_input;
    pMotors->_pitch_radio_passthrough = pitch_input;
    pMotors->_throttle_radio_passthrough = throttle_input;
    pMotors->_yaw_radio_passthrough = yaw_input;
}

// set loop rate. Used to support loop rate as a parameter
void Motors_set_loop_rate(Motors_HandleTypeDef *pMotors, uint16_t loop_rate)
{
    pMotors->_loop_rate = loop_rate;
}

// return the roll factor of any motor, this is used for tilt rotors and tail sitters
// using copter motors for forward flight
float Motors_get_roll_factor(Motors_HandleTypeDef *pMotors, uint8_t i)
{
    return 0.0f;
}

// This function required for tradheli. Tradheli initializes targets when going from unarmed to armed state.
// This function is overriden in motors_heli class.   Always true for multicopters.
bool Motors_init_targets_on_arming(Motors_HandleTypeDef *pMotors)
{
    return true;
}

motors_pwm_type Motors_get_pwm_type(Motors_HandleTypeDef *pMotors)
{
    return (motors_pwm_type)pMotors->_pwm_type; 
}

void Motors_rc_write(Motors_HandleTypeDef *pMotors, uint8_t chan, uint16_t pwm)
{
    if (chan < GP_MOTORS_MAX_NUM_MOTORS) {
        pMotors->_motor_output_pwm[chan] = pwm;
    }

    Aux_servo_function_t function = srv_channels_get_motor_function(chan);
    if ((1U<<chan) & pMotors->_motor_pwm_range_mask) {
        // note that PWM_MIN/MAX has been forced to 1000/2000
        srv_channels_set_output_scaled(function, pwm - 1000);
    } else {
        srv_channels_set_output_pwm(function, pwm);
    }
}

// add a motor to the motor map
void Motors_add_motor_num(Motors_HandleTypeDef *pMotors, int8_t motor_num)
{
    uint8_t motor_quantity = motor_num + 1;

    if (motor_quantity > pMotors->_motor_quantity) {
        pMotors->_motor_quantity = motor_quantity;
    }

    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < GP_MOTORS_MAX_NUM_MOTORS) {
        uint8_t chan;
        Aux_servo_function_t function = srv_channels_get_motor_function(motor_num);
        srv_channels_set_aux_channel_default(function, motor_num);
        if (!srv_channels_find_channel(function, &chan)) {
            mavproxy_send_statustext(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
        }
    }
}

// check initialisation succeeded
bool Motors_initialised_ok(Motors_HandleTypeDef *pMotors)
{
    return pMotors->_initialised_ok;
}

void Motors_set_initialised_ok(Motors_HandleTypeDef *pMotors, bool val)
{
    pMotors->_initialised_ok = val;
}

/*
  map an internal motor mask to real motor mask, accounting for
  SERVOn_FUNCTION mappings, and allowing for multiple outputs per
  motor number
 */
uint32_t Motors_motor_mask_to_srv_channel_mask(uint32_t mask)
{
    uint32_t mask2 = 0;
    for (uint8_t i = 0; i < 32; i++) {
        uint32_t bit = 1UL << i;
        if (mask & bit) {
            Aux_servo_function_t function = srv_channels_get_motor_function(i);
            mask2 |= srv_channels_get_output_channel_mask(function);
        }
    }
    return mask2;
}

/*
  set frequency of a set of channels
 */
void Motors_rc_set_freq(Motors_HandleTypeDef *pMotors,uint32_t mask, uint16_t freq_hz)
{
    if (freq_hz > 50) {
        pMotors->_motor_fast_mask |= mask;
    }

    mask = Motors_motor_mask_to_srv_channel_mask(mask);
    srv_hal_set_freq(mask, freq_hz);

    switch ((motors_pwm_type)(pMotors->_pwm_type)) {
    case PWM_TYPE_ONESHOT:
        if (freq_hz > 50 && mask != 0) {
            srv_hal_set_output_mode(mask, MODE_PWM_ONESHOT);
        }
        break;
    case PWM_TYPE_ONESHOT125:
        if (freq_hz > 50 && mask != 0) {
            srv_hal_set_output_mode(mask, MODE_PWM_ONESHOT125);
        }
        break;
    case PWM_TYPE_BRUSHED:
        srv_hal_set_output_mode(mask, MODE_PWM_BRUSHED);
        break;
    case PWM_TYPE_DSHOT150:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT150);
        break;
    case PWM_TYPE_DSHOT300:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT300);
        break;
    case PWM_TYPE_DSHOT600:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT600);
        break;
    case PWM_TYPE_DSHOT1200:
        srv_hal_set_output_mode(mask, MODE_PWM_DSHOT1200);
        break;
    case PWM_TYPE_PWM_RANGE:
        /*
          this is a motor output type for multirotors which honours
          the SERVOn_MIN/MAX values per channel
         */
        pMotors->_motor_pwm_range_mask |= mask;
        for (uint8_t i=0; i<16; i++) {
            if ((1U<<i) & mask) {
                srv_channels_set_range(srv_channels_get_motor_function(i), 1000);
            }
        }
        srv_hal_set_output_mode(mask, MODE_PWM_NORMAL);
        break;
    default:
        srv_hal_set_output_mode(mask, MODE_PWM_NORMAL);
        break;
    }
}

/*------------------------------------test------------------------------------*/


